#include <sstream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "ultra_band_wave_ladar/Sensor_mmRad.hpp"
#include "ultra_band_wave_ladar/Sensor_Ultra.hpp"
#include "ultra_band_wave_ladar/Sensor_RadLogic.hpp"
#include "ultra_band_wave_ladar/ObstacleInfo.h"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "Radsensor Talk to motion");
  ros::NodeHandle nh;

  ros::Publisher pub_mmRad;
  pub_mmRad = nh.advertise<ultra_band_wave_ladar::ObstacleInfo>("mmRad Obstacle Alarm", 1000);

  ros::Publisher pub_Ultra_back;
  pub_Ultra_back = nh.advertise<ultra_band_wave_ladar::ObstacleInfo>("UltraRad back Obstacle Alarm", 1000);

  ros::Publisher pub_Ultra_left;
  pub_Ultra_left = nh.advertise<ultra_band_wave_ladar::ObstacleInfo>("UltraRad letf Obstacle Alarm", 1000);

  ros::Publisher pub_Ultra_right;
  pub_Ultra_right = nh.advertise<ultra_band_wave_ladar::ObstacleInfo>("UltraRad right Obstacle Alarm", 1000);

  ros::Rate loop_rate(10);

  while (ros::ok())
  {
    if (sensorinfo.mmRad_Alarm)
      pub_mmRad.publish(sensorinfo.mmRad_front);
    if (sensorinfo.mmRad_Alarm)
      pub_Ultra_back.publish(sensorinfo.UltraRad_back);
    if (sensorinfo.mmRad_Alarm)
      pub_Ultra_left.publish(sensorinfo.UltraRad_left);
    if (sensorinfo.mmRad_Alarm)
      pub_Ultra_right.publish(sensorinfo.UltraRad_right);

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
